
#include <zephyr.h>
#include <sys/printk.h>
#include <kernel_structs.h>
#include <string.h>
#include <stdlib.h>
#include <sensor.h>
#include <app_memory/app_memdomain.h>
#include <sys/util.h>

#define WHEEL_SIZE 26
#define IMOD(a, b) ((a + b) % WHEEL_SIZE)

void enc(void);
void pt(void);
void ct(void);
void pt_sub(void);

#define _app_user_d K_APP_DMEM(part0)
#define _app_user_b K_APP_BMEM(part0)

#define _app_red_d K_APP_DMEM(part1)
#define _app_red_b K_APP_BMEM(part1)

#define _app_enc_d K_APP_DMEM(part2)
#define _app_enc_b K_APP_BMEM(part2)

#define _app_blk_d K_APP_DMEM(part3)
#define _app_blk_b K_APP_BMEM(part3)

#define _app_ct_d K_APP_DMEM(part4)
#define _app_ct_b K_APP_BMEM(part4)

/*
 * Constant
 */

#define STACKSIZE 1024

#define PRIORITY 7

#define BYTE unsigned char


#define START_WHEEL {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, \
	12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25}
#define START_WHEEL2 {6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, \
	17, 18, 19, 20, 21, 22, 23, 24, 25, 5, 0, 4, 1, 3, 2}
#define REFLECT {1, 0, 3, 2, 5, 4, 7, 6, 9, 8, 11, 10, 13, 12, \
	15, 14, 17, 16, 19, 18, 21, 20, 23, 22, 25, 24}

/* the following definition name prefix is to avoid a conflict */
#define SAMP_BLOCKSIZE 50


/*
 *  The memory partitions have been named to simplify
 * the definition of variables.  A possible alternative
 * is using one source file per thread and implementing
 * a objcopy to rename the data and bss section for the
 * thread to the partiotion name.
 */

/* prepare the memory partition structures  */
FOR_EACH(K_APPMEM_PARTITION_DEFINE, part0, part1, part2, part3, part4);
extern struct k_mem_partition z_malloc_partition;
/* prepare the memory domain structures  */
struct k_mem_domain dom0, dom1, dom2;
/* each variable starts with a name defined in main.h
 * the names are symbolic for the memory partitions
 * purpose.
 */
_app_red_b BYTE fBUFIN;
_app_red_b BYTE BUFIN[63];

_app_blk_b BYTE fBUFOUT;
_app_blk_b BYTE BUFOUT[63];

/* declare and set wheel and reflector  */
/* To use add definition ALTMSG */
#ifdef ALTMSG
_app_enc_d BYTE W1[26] = START_WHEEL;
#else
_app_enc_d BYTE W1[26] = START_WHEEL2;
#endif
_app_enc_d BYTE W2[26] = START_WHEEL;
_app_enc_d BYTE W3[26] = START_WHEEL;
_app_enc_d BYTE R[26] = REFLECT;

_app_enc_b int IW1;
_app_enc_b int IW2;
_app_enc_b int IW3;

/*
 *   calculated by the enc thread at init and when the wheels
 *   change.
 */
_app_enc_b BYTE W1R[26];
_app_enc_b BYTE W2R[26];
_app_enc_b BYTE W3R[26];

/*
 *	sync threads
 */
K_SEM_DEFINE(allforone, 0, 3);

struct k_thread enc_thread;
K_THREAD_STACK_DEFINE(enc_stack, STACKSIZE);

struct k_thread pt_thread;
K_THREAD_STACK_DEFINE(pt_stack, STACKSIZE);

struct k_thread ct_thread;
K_THREAD_STACK_DEFINE(ct_stack, STACKSIZE);

struct k_thread pt_sub_thread;
K_THREAD_STACK_DEFINE(pt_sub_stack, 60);


_app_enc_d char encMSG[] = "ENC!\n";
_app_enc_d int enc_state = 1;
_app_enc_b char enc_pt[50];  /* Copy form shared pt */
_app_enc_b char enc_ct[50]; /* Copy to shared ct */

_app_user_d char ptMSG[] = "PT: message to encrypt\n";

/* encrypted message when W1 = START_WHEEL */
/* to use add definition ALTMSG  */
#ifdef ALTMSG
_app_user_d char ptMSG2[] = "nfttbhfspfmdqzos\n";
#else
/* encrypted message when W1 = START_WHEEL2 */
_app_user_d char ptMSG2[] = "ofttbhfspgmeqzos\n";
#endif
_app_ct_d char ctMSG[] = "CT!\n";

 short char_to_index(char c);
 char index_to_char(short i);


char enig_enc(char pt)
{
	short tmpIndex;
	char ct;
#ifdef DBUG
	printk("\nEE PT: %c, %02x\n", pt, pt);
	printk("Index: %d, %d, %d\n", IW1, IW2, IW3);
#endif
	tmpIndex = char_to_index(pt);
#ifdef DBUG
	printk("EE   : %02x\n", tmpIndex);
#endif
	/* if error return  */
	if (tmpIndex == -1) {
		return (char)0xFF;
	}

	tmpIndex = (W1[IMOD(IW1, tmpIndex)] + WHEEL_SIZE - IW1) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i1: %02x\n", tmpIndex);
#endif
	tmpIndex = (W2[IMOD(IW2, tmpIndex)] +  WHEEL_SIZE - IW2) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i2: %02x\n", tmpIndex);
#endif
	tmpIndex = (W3[IMOD(IW3, tmpIndex)] + WHEEL_SIZE - IW3) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i3: %02x\n", tmpIndex);
#endif
	tmpIndex = R[tmpIndex];
#ifdef DBUG
	printk("EE  r: %02x\n", tmpIndex);
#endif
	tmpIndex = (W3R[IMOD(IW3, tmpIndex)] + WHEEL_SIZE - IW3) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i3: %02x\n", tmpIndex);
#endif
	tmpIndex = (W2R[IMOD(IW2, tmpIndex)] + WHEEL_SIZE - IW2) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i2: %02x\n", tmpIndex);
#endif
	tmpIndex = (W1R[IMOD(IW1, tmpIndex)] + WHEEL_SIZE - IW1) % WHEEL_SIZE;
#ifdef DBUG
	printk("EE i1: %02x\n", tmpIndex);
#endif

	ct = index_to_char(tmpIndex);
#ifdef DBUG
	printk("EE CT: %02x\n", ct);
#endif
	/* test ct value or just return error ? */
	update_wheel_index();
	return ct;
}

/*
 * calc reverse path for wheel
 *  this simplifies the reverse path calculation
 * Return: 1:ok -1 error
 */
int calc_rev_wheel(BYTE *wheel, BYTE *backpath)
{

	int i;

	for (i = 0; i < WHEEL_SIZE; i++) {
		if (wheel[i] >= WHEEL_SIZE) {
			return -1;
		}
		backpath[wheel[i]] = i;
	}
	return 1;
}


/*
 * convert a-z to 0-25
 */
 short char_to_index(char c)
{
	if (c < 'a' || c > 'z') {
		return -1;
	}
	return (short)(c - 'a');
}

/*
 *  convert from a index 0-25 to a-z
 */
char index_to_char(short i)
{
	if (i < 0 || i > 25) {
		return 0xFF;
	}
	return (char)((short)'a' + i);
}

/*
 *  basic update to wheels based on full rotation
 *  of prior wheel.  This could be modified to change
 *  the direction of rotation or order of updates
 */
void update_wheel_index(void)
{
	IW1++;
	if (IW1 >= WHEEL_SIZE) {
		IW1 %= WHEEL_SIZE;
		IW2++;
	}
	if (IW2 >= WHEEL_SIZE) {
		IW2 %= WHEEL_SIZE;
		IW3++;
	}
	if (IW3 >= WHEEL_SIZE) {
		IW3 %= WHEEL_SIZE;
	}

}

void use_space_test(void)
{
	struct k_mem_partition *dom1_parts[] = {&part2, &part1, &part3};
	struct k_mem_partition *dom2_parts[] = {&part4, &part3};
	struct k_mem_partition *dom0_parts[] = {&part0, &part1, &z_malloc_partition};
	k_tid_t tPT, tENC, tCT;

	k_thread_access_grant(k_current_get(), &allforone);

	/*
	 * create an enc thread init the memory domain and add partitions
	 * then add the thread to the domain.
	 */
	tENC = k_thread_create(&enc_thread, enc_stack, STACKSIZE,
			(k_thread_entry_t)enc, NULL, NULL, NULL,
			-1, K_USER,
			K_FOREVER);
	k_thread_access_grant(tENC, &allforone);
	/* use K_FOREVER followed by k_thread_start*/
	printk("ENC Thread Created %08X\n", (unsigned int) tENC);
	k_mem_domain_init(&dom1, 3, dom1_parts);
	printk("Partitions added to dom1\n");
	k_mem_domain_add_thread(&dom1, tENC);
	printk("dom1 Created\n");

	printk("super malloc %p\n", malloc(16));

	tPT = k_thread_create(&pt_thread, pt_stack, STACKSIZE,
			(k_thread_entry_t)pt, NULL, NULL, NULL,
			-1, K_USER,
			K_FOREVER);
	struct device *dev = device_get_binding("DHT11"); //根据Device name获取驱动
	k_thread_access_grant(tPT, dev);
	k_thread_access_grant(tPT, &pt_sub_thread);
	k_thread_access_grant(tPT, pt_sub_stack);
	k_thread_access_grant(tPT, &allforone);
	printk("PT Thread Created %08X\n", (unsigned int) tPT);
	k_mem_domain_init(&dom0, 2, dom0_parts);
	k_mem_domain_add_thread(&dom0, tPT);
	printk("dom0 Created\n");
	//extern struct sys_mem_pool z_malloc_mem_pool;
	//k_thread_access_grant(tPT, &z_malloc_mem_pool)

	tCT = k_thread_create(&ct_thread, ct_stack, STACKSIZE,
			(k_thread_entry_t)ct, NULL, NULL, NULL,
			-1, K_USER,
			K_FOREVER);
	k_thread_access_grant(tCT, &allforone);
	printk("CT Thread Created %08X\n", (unsigned int) tCT);
	k_mem_domain_init(&dom2, 2, dom2_parts);
	k_mem_domain_add_thread(&dom2, tCT);
	printk("dom2 Created\n");

	k_thread_start(&enc_thread);
	/* need to start all three threads.  let enc go first to perform init step */

	printk("ENC thread started\n");
	k_thread_start(&pt_thread);
	printk("PT thread started\n");

	k_thread_start(&ct_thread);
	k_sem_give(&allforone);
	printk("CT thread started\n");
	k_thread_abort(k_current_get());

}



/*
 * The enc thread.
 * Function: initialize the the simulation of the wheels.
 * Copy memory from pt thread and encrypt to a local buffer
 * then copy to the ct thread.
 */
void enc(void)
{

	int index, index_out;
	if (enc_state == 1) {
		fBUFIN = 0; /* clear flags */
		fBUFOUT = 0;
		calc_rev_wheel((BYTE *) &W1, (BYTE *)&W1R);
		calc_rev_wheel((BYTE *) &W2, (BYTE *)&W2R);
		calc_rev_wheel((BYTE *) &W3, (BYTE *)&W3R);
		IW1 = 0;
		IW2 = 0;
		IW3 = 0;
		enc_state = 0;
	}

	while (1) {
		k_sem_take(&allforone, K_FOREVER);
		if (fBUFIN == 1) { /* 1 is process text */
			printk("ENC Thread Received Data\n");
			/* copy message form shared mem and clear flag */
			memcpy(&enc_pt, BUFIN, SAMP_BLOCKSIZE);
			printk("ENC PT MSG: %s\n", (char *)&enc_pt);
			fBUFIN = 0;
			/* reset wheel: probably better as a flag option  */
			IW1 = 7;
			IW2 = 2;
			IW3 = 3;
			/* encode */
			memset(&enc_ct, 0, SAMP_BLOCKSIZE); /* clear memory */
			for (index = 0, index_out = 0; index < SAMP_BLOCKSIZE; index++) {
				if (enc_pt[index] == '\0') {
					enc_ct[index_out] = '\0';
					break;
				}
				if (enc_pt[index] >= 'a' && enc_pt[index] <= 'z') {
					enc_ct[index_out] = (BYTE)enig_enc((BYTE) enc_pt[index]);
					index_out++;
				}
			}
			/* test for CT flag */
			while (fBUFOUT != 0) {
				k_sleep(100);
			}
			/* ct thread has cleared the buffer */
			memcpy(&BUFOUT, &enc_ct, SAMP_BLOCKSIZE);
			fBUFOUT = 1;

		}
		k_sem_give(&allforone);
	}
}

/*
 * the pt function pushes data to the enc thread.
 * It can be extended to receive data from a serial port
 * and pass the data to enc
 */


void pt(void)
{

	k_sleep(2000);
	k_thread_create(&pt_sub_thread, pt_sub_stack, 60,
			(k_thread_entry_t)pt_sub, NULL, NULL, NULL,
			-1, K_USER,
			K_FOREVER);
	
	k_thread_start(&pt_sub_thread);
	
	while (1) {
		k_sem_take(&allforone, K_FOREVER);

	struct sensor_value temp, humidity;
	struct device *dev1 = device_get_binding("DHT11"); 
	//printk("dev %p name %s\n", dev, dev->config->name);
	sensor_sample_fetch(dev1);								  //从dh11读取数据
	sensor_channel_get(dev1, SENSOR_CHAN_AMBIENT_TEMP, &temp); //读取温度
	sensor_channel_get(dev1, SENSOR_CHAN_HUMIDITY, &humidity); //读取湿度
	printk("temp: %d.%06d; humidity: %d.%06d\n",
		   temp.val1, temp.val2, humidity.val1, humidity.val2);


		if (fBUFIN == 0) { /* send message to encode */
			printk("\nPT Sending Message 1\n");
			memset(&BUFIN, 0, SAMP_BLOCKSIZE);
			memcpy(&BUFIN, &ptMSG, sizeof(ptMSG));
/* strlen should not be used if user provided data, needs a max length set  */
			fBUFIN = 1;
		}
		k_sem_give(&allforone);
		k_sem_take(&allforone, K_FOREVER);
		if (fBUFIN == 0) { /* send message to decode  */
			printk("\nPT Sending Message 1'\n");
			memset(&BUFIN, 0, SAMP_BLOCKSIZE);
			memcpy(&BUFIN, &ptMSG2, sizeof(ptMSG2));
			fBUFIN = 1;
		}
		k_sem_give(&allforone);
		k_sleep(5000);
	}
}

/*
 * CT waits for fBUFOUT = 1 then copies
 * the message clears the flag and prints
 */
void ct(void)
{

	char tbuf[60];

	while (1) {
		k_sem_take(&allforone, K_FOREVER);
		if (fBUFOUT == 1) {
			printk("CT Thread Receivedd Message\n");
			memset(&tbuf, 0, sizeof(tbuf));
			memcpy(&tbuf, BUFOUT, SAMP_BLOCKSIZE);
			fBUFOUT = 0;
			printk("CT MSG: %s\n", (char *)&tbuf);
		}
		k_sem_give(&allforone);
	}
}


void pt_sub(void)
{
	while(1)
	{
		//k_sleep(100);
	}
}
